home *** CD-ROM | disk | FTP | other *** search
/ Compendium Deluxe 1 / LSD Compendium Deluxe 1.iso / a / programming / assembly / cordic.lha / cordic1.code / text0000.txt < prev   
Encoding:
Text File  |  1989-10-24  |  9.8 KB  |  390 lines
  1. >From turk@apple.UUCP Wed Feb 17 21:19:06 1988
  2. Path: leah!itsgw!nysernic!rutgers!ucla-cs!cit-vax!elroy!ames!pasteur!ucbvax!hplabs!nsc!voder!apple!turk
  3. From: turk@apple.UUCP (Ken "Turk" Turkowski)
  4. Newsgroups: comp.graphics
  5. Subject: Re: CORDICS: Is Ken Turkowski there? (was: Fractals...Jim Cathey where are you?)
  6. Summary: CORDIC derivation and code enclosed
  7. Keywords: CORDIC, fixed-point trigonometric functions
  8. Message-ID: <7433@apple.UUCP>
  9. Date: 18 Feb 88 02:19:06 GMT
  10. References: <6019@iuvax.UUCP> <2479@orca.TEK.COM>
  11. Reply-To: turk@apple.UUCP (Ken "Turk" Turkowski)
  12. Organization: Advanced Technology Graphics, Apple Computer, Cupertino, CA, USA
  13. Lines: 373
  14.  
  15. In article <2479@orca.TEK.COM> brucec@orca.UUCP (Bruce Cohen) writes:
  16. >In article <6019@iuvax.UUCP> viking@iuvax writes:
  17. >>
  18. >>The major enhancements that Jim added to the program were to recode it in
  19. >>C and to use "a CORDIC rotator instead of trig functions".  I want to talk
  20. >>with Jim about his program....where is he?!
  21. >>
  22. >>Alternately, does anyone know anything about "CORDIC rotators"?  I'd like to
  23. >>see the source to his modification of Michiel's program.  The algorithm used
  24. >>was based on one developed by LucasFilm Ltd. and published in the Sept. '84
  25. >>Scientific American.
  26.  
  27. The idea is really simple.  A rotation can be expressed as:
  28.  
  29.     [ x' ]   [ cos t   -sin y ] [ x ]
  30.     [ y' ] = [ sin t    cos t ] [ y ]
  31.  
  32. Factor out cos t, giving:
  33.  
  34.     [ x' ]         [   1     -tan y ] [ x ]
  35.     [ y' ] = cos t [ tan t      1   ] [ y ]
  36.  
  37. Now let t be expressed as a signed sum of 
  38.                 -1  -i
  39.     t = Sum { d  tan   2   },   where d  = {+1, -1}
  40.          i     i                       i
  41.  
  42. This yields (please forgive the cruddy typesetting; Mathtype doesn't work
  43. on UNIX):
  44.  
  45.                                           -i
  46.     [ x' ]                    [ 1     -d 2   ] [ x ]
  47.     [    ]                    [         i    ] [   ]
  48.     [    ] = Sum { C  } Sum { [    -i        ] [   ] }
  49.     [ y' ]    i     i    i    [ d 2      1   ] [ y ]
  50.                      i
  51.  
  52. The first sum is a constant.  The second sum is a series of shifts and
  53. adds (or subtracts).
  54.  
  55.  
  56. This is illustrated by the following working code, which performs all of the
  57. following operations:
  58.     rotate a vector
  59.     polar to rectangular conversion
  60.     rectangular to polar conversion
  61.     simultaneous calculation of sine and cosine.
  62.     atan2()
  63. Note that the normalization and denormalization is done mainly to increase
  64. accuracy; if you're more concerned about speed, you'd probably hack this
  65. out entirely or just shift by a fixed amount.
  66.  
  67. You can do similar sorts of things with hyperbolic functions, thereby
  68. being able to do exponential and logarithms.  I have never written such
  69. code, although I would like to have it, especially to calculate gamma
  70. correction tables.
  71.  
  72. ------------------------------------------------------------
  73.  
  74. # define RADIANS 1
  75. # define COSCALE 0x22c2dd1c    /* 0.271572 */
  76.  
  77. static long arctantab[32] = {
  78. # ifdef DEGREES        /* MS 10 integral bits */
  79. # define QUARTER (90 << 22)
  80.     266065460, 188743680, 111421900, 58872272, 29884485, 15000234, 7507429,
  81.     3754631, 1877430, 938729, 469366, 234683, 117342, 58671, 29335, 14668,
  82.     7334, 3667, 1833, 917, 458, 229, 115, 57, 29, 14, 7, 4, 2, 1, 0, 0, 
  83. # else
  84. # ifdef RADIANS    /* MS 4 integral bits */
  85. # define QUARTER ((int)(3.141592654 / 2.0 * (1 << 28)))
  86.     297197971, 210828714, 124459457, 65760959, 33381290, 16755422, 8385879,
  87.     4193963, 2097109, 1048571, 524287, 262144, 131072, 65536, 32768, 16384,
  88.     8192, 4096, 2048, 1024, 512, 256, 128, 64, 32, 16, 8, 4, 2, 1, 0, 0, 
  89. # else
  90. # define BRADS 1
  91. # define QUARTER (1 << 30)
  92.     756808418, 536870912, 316933406, 167458907, 85004756, 42667331,
  93.     21354465, 10679838, 5340245, 2670163, 1335087, 667544, 333772, 166886,
  94.     83443, 41722, 20861, 10430, 5215, 2608, 1304, 652, 326, 163, 81, 41,
  95.     20, 10, 5, 3, 1, 1, 
  96. # endif
  97. # endif
  98. };
  99.  
  100.  
  101. /* To rotate a vector through an angle of theta, we calculate:
  102.  *
  103.  *    x' = x cos(theta) - y sin(theta)
  104.  *    y' = x sin(theta) + y cos(theta)
  105.  *
  106.  * The rotate() routine performs multiple rotations of the form:
  107.  *
  108.  *    x[i+1] = cos(theta[i]) { x[i] - y[i] tan(theta[i]) }
  109.  *    y[i+1] = cos(theta[i]) { y[i] + x[i] tan(theta[i]) }
  110.  *
  111.  * with the constraint that tan(theta[i]) = pow(2, -i), which can be
  112.  * implemented by shifting. We always shift by either a positive or
  113.  * negative angle, so the convergence has the ringing property. Since the
  114.  * cosine is always positive for positive and negative angles between -90
  115.  * and 90 degrees, a predictable magnitude scaling occurs at each step,
  116.  * and can be compensated for instead at the end of the iterations by a
  117.  * composite scaling of the product of all the cos(theta[i])'s.
  118.  */
  119.  
  120. static
  121. pseudorotate(px, py, theta)
  122. long *px, *py;
  123. register long theta;
  124. {
  125.     register int i;
  126.     register long x, y, xtemp;
  127.     register long *arctanptr;
  128.  
  129.     x = *px;
  130.     y = *py;
  131.  
  132.     /* Get angle between -90 and 90 degrees */
  133.     while (theta < -QUARTER) {
  134.         x = -x;
  135.         y = -y;
  136.         theta += 2 * QUARTER;
  137.     }
  138.     while (theta > QUARTER) {
  139.         x = -x;
  140.         y = -y;
  141.         theta -= 2 * QUARTER;
  142.     }
  143.  
  144.     /* Initial pseudorotation, with left shift */
  145.     arctanptr = arctantab;
  146.     if (theta < 0) {
  147.         xtemp = x + (y << 1);
  148.         y     = y - (x << 1);
  149.         x = xtemp;
  150.         theta += *arctanptr++;
  151.     }
  152.     else {
  153.         xtemp = x - (y << 1);
  154.         y     = y + (x << 1);
  155.         x = xtemp;
  156.         theta -= *arctanptr++;
  157.     }
  158.  
  159.     /* Subsequent pseudorotations, with right shifts */
  160.     for (i = 0; i <= 28; i++) {
  161.         if (theta < 0) {
  162.         xtemp = x + (y >> i);
  163.         y     = y - (x >> i);
  164.         x = xtemp;
  165.         theta += *arctanptr++;
  166.         }
  167.         else {
  168.         xtemp = x - (y >> i);
  169.         y     = y + (x >> i);
  170.         x = xtemp;
  171.         theta -= *arctanptr++;
  172.         }
  173.     }
  174.  
  175.     *px = x;
  176.     *py = y;
  177. }
  178.  
  179.  
  180. static
  181. pseudopolarize(argx, argy)
  182. long *argx, *argy;
  183. {
  184.     register long theta;
  185.     register long yi, i;
  186.     register long x, y;
  187.     register long *arctanptr;
  188.  
  189.     x = *argx;
  190.     y = *argy;
  191.  
  192.     /* Get the vector into the right half plane */
  193.     theta = 0;
  194.     if (x < 0) {
  195.         x = -x;
  196.         y = -y;
  197.         theta = 2 * QUARTER;
  198.     }
  199.  
  200.     if (y > 0)
  201.         theta = - theta;
  202.     
  203.     arctanptr = arctantab;
  204.     if (y < 0) {    /* Rotate positive */
  205.         yi = y + (x << 1);
  206.         x = x - (y << 1);
  207.         y = yi;
  208.         theta -= *arctanptr++;    /* Subtract angle */
  209.     }
  210.     else {        /* Rotate negative */
  211.         yi = y - (x << 1);
  212.         x = x + (y << 1);
  213.         y = yi;
  214.         theta += *arctanptr++;    /* Add angle */
  215.     }
  216.  
  217.     for (i = 0; i <= 28; i++) {
  218.         if (y < 0) {    /* Rotate positive */
  219.         yi = y + (x >> i);
  220.         x = x - (y >> i);
  221.         y = yi;
  222.         theta -= *arctanptr++;
  223.         }
  224.         else {        /* Rotate negative */
  225.         yi = y - (x >> i);
  226.         x = x + (y >> i);
  227.         y = yi;
  228.         theta += *arctanptr++;
  229.         }
  230.     }
  231.  
  232.     *argx = x;
  233.     *argy = theta;
  234. }
  235.  
  236.  
  237. /* Fxprenorm() block normalizes the arguments to a magnitude suitable for
  238.  * CORDIC pseudorotations.  The returned value is the block exponent.
  239.  */
  240. static int
  241. fxprenorm(argx, argy)
  242. long *argx, *argy;
  243. {
  244.     register long x, y;
  245.     register int shiftexp;
  246.     int signx, signy;
  247.  
  248.     shiftexp = 0;        /* Block normalization exponent */
  249.     signx = signy = 1;
  250.  
  251.     if ((x = *argx) < 0) {
  252.         x = -x; signx = -signx;
  253.     }
  254.     if ((y = *argy) < 0) {
  255.         y = -y; signy = -signy;
  256.     }
  257.     /* Prenormalize vector for maximum precision */
  258.     if (x < y) {    /* |y| > |x| */
  259.         while (y < (1 << 27)) {
  260.         x <<= 1; y <<= 1; shiftexp--;
  261.         }
  262.         while (y > (1 << 28)) {
  263.         x >>= 1; y >>= 1; shiftexp++;
  264.         }
  265.     }
  266.     else {        /* |x| > |y| */
  267.         while (x < (1 << 27)) {
  268.         x <<= 1; y <<= 1; shiftexp--;
  269.         }
  270.         while (x > (1 << 28)) {
  271.         x >>= 1; y >>= 1; shiftexp++;
  272.         }
  273.     }
  274.  
  275.     *argx = (signx < 0) ? -x : x;
  276.     *argy = (signy < 0) ? -y : y;
  277.     return(shiftexp);
  278. }
  279.  
  280.  
  281. /* Return a unit vector corresponding to theta.
  282.  * sin and cos are fixed-point fractions.
  283.  */
  284. fxunitvec(cos, sin, theta)
  285. long *cos, *sin, theta;
  286. {
  287.     *cos = COSCALE >> 1;    /* Save a place for the integer bit */
  288.     *sin = 0;
  289.     pseudorotate(cos, sin, theta);
  290.  
  291.     /* Saturate results to fractions less than 1, shift out integer bit */
  292.     if (*cos >= (1 << 30)) 
  293.         *cos = 0x7FFFFFFF;        /* Just shy of 1 */
  294.     else if (*cos <= -(1 << 30))
  295.         *cos = 0x80000001;        /* Just shy of -1*/
  296.     else
  297.         *cos <<= 1;
  298.  
  299.     if (*sin >= (1 << 30)) 
  300.         *sin = 0x7FFFFFFF;        /* Just shy of 1 */
  301.     else if (*sin <= -(1 << 30))
  302.         *sin = 0x80000001;        /* Just shy of -1*/
  303.     else
  304.         *sin <<= 1;
  305. }
  306.  
  307.  
  308. /* Fxrotate() rotates the vector (argx, argy) by the angle theta. */
  309. fxrotate(argx, argy, theta)
  310. long *argx, *argy;
  311. long theta;
  312. {
  313.     register long x, y;
  314.     int shiftexp;
  315.     long frmul();
  316.  
  317.     if (((*argx || *argy) == 0) || (theta == 0))
  318.         return;
  319.  
  320.     shiftexp = fxprenorm(argx, argy);  /* Prenormalize vector */
  321.     pseudorotate(argx, argy, theta);   /* Perform CORDIC pseudorotation */
  322.     x = frmul(*argx, COSCALE);    /* Compensate for CORDIC enlargement */
  323.     y = frmul(*argy, COSCALE);
  324.     if (shiftexp < 0) {        /* Denormalize vector */
  325.         *argx = x >> -shiftexp;
  326.         *argy = y >> -shiftexp;
  327.     }
  328.     else {
  329.         *argx = x << shiftexp;
  330.         *argy = y << shiftexp;
  331.     }
  332. }
  333.  
  334.  
  335. fxatan2(x, y)
  336. long x, y;
  337. {
  338.     if ((x || y) == 0)
  339.         return(0);
  340.     fxprenorm(&x, &y);    /* Prenormalize vector for maximum precision */
  341.     pseudopolarize(&x, &y);    /* Convert to polar coordinates */
  342.     return(y);
  343. }
  344.  
  345.  
  346. fxpolarize(argx, argy)
  347. long *argx, *argy;
  348. {
  349.     int shiftexp;
  350.     long frmul();
  351.  
  352.     if ((*argx || *argy) == 0) {
  353.         *argx = 0;    /* Radius */
  354.         *argy = 0;    /* Angle */
  355.         return;
  356.     }
  357.  
  358.     /* Prenormalize vector for maximum precision */
  359.     shiftexp = fxprenorm(argx, argy);
  360.     /* Perform CORDIC conversion to polar coordinates */
  361.     pseudopolarize(argx, argy);
  362.     /* Scale radius to undo pseudorotation enlargement factor */
  363.     *argx = frmul(*argx, COSCALE);
  364.     /* Denormalize radius */
  365.     *argx = (shiftexp < 0) ? (*argx >> -shiftexp) : (*argx << shiftexp);
  366. }
  367.  
  368.  
  369. # ifdef vax
  370. long
  371. frmul(a, b)        /* Just for testing */
  372. long a, b;
  373. {
  374.     /* This routine should be written in assembler to calculate the
  375.      * high part of the product, i.e. the product when the operands
  376.      * are considered fractions.
  377.      */
  378.     return((a >> 16) * (b >> 15));
  379. }
  380. # endif
  381.  
  382. ##### This is the end of my posting #####
  383. -- 
  384. Ken Turkowski @ Apple Computer, Inc., Cupertino, CA
  385. UUCP: {mtxinu,sun,decwrl,nsc,voder}!apple!turk
  386. CSNET: turk@Apple.CSNET
  387. ARPA: turk%Apple@csnet-relay.ARPA
  388.  
  389.  
  390.